package edu.wpi.first.wpilibj.templates.commands;

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 *
 * @author Super NURDs
 */

public class DoTurnAngle extends CommandBase {
  
    private double speed;
    private double angle;
    
    public DoTurnAngle(double s, double a) {
        if(a < 0) {
            s = -s;
        }
        this.speed=s;
        this.angle=a;
         // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
        requires(myDriveTrain);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
        myDriveTrain.resetGyro();
   }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
       myDriveTrain.setSpeed(speed, -speed);
    }
                
    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        double gyroAngle = myDriveTrain.getGyroAngle();
        if (angle >= 0.0) {
            if ((angle - gyroAngle) <= 0.0) {
                return (true);
            }
            else {
                return (false);
            }
        }
        else {
            if ((angle - gyroAngle) >= 0.0) {
                return (true);
            }
            else {
                return (false);
            }     
        }
    }

    // Called once after isFinished returns true
    protected void end() {
        myDriveTrain.stop();
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
        end();
    }
}